/*
 * Copyright (c) 2011, The Bishops Arm Project All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.  Redistributions in binary
 * form must reproduce the above copyright notice, this list of conditions and
 * the following disclaimer in the documentation and/or other materials
 * provided with the distribution.  Neither the name of the The Bishops Arm
 * Project nor the names of its contributors may be used to endorse or promote
 * products derived from this software without specific prior written
 * permission.

 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

package se.joinenea.bishopsarm;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.EnumSet;
import java.util.Timer;
import java.util.TimerTask;

public class NXTMotorController {
    private static final int NXTPORT_C = 2;
    private static final int NXTPORT_B = 1;
    private final NXTBot parent;
    private int leftSpeed;
    private int rightSpeed;
    private boolean initiated;

    public NXTMotorController(NXTBot nxtBot) {
        this.parent = nxtBot;
        this.initiated = false;
    }

    public void initiate() {
        final Timer timer = new Timer();
        timer.schedule(new TimerTask() {
            @Override
            public void run() {
                try {
                    synchronized (this) {
                        setOutputState(NXTPORT_B, leftSpeed,
                                EnumSet.of(MotorMode.MOTORON),
                                MotorRegulationMode.MOTOR_SPEED, 0,
                                MotorRunState.RUNNING, 0);
                        setOutputState(NXTPORT_C, rightSpeed,
                                EnumSet.of(MotorMode.MOTORON),
                                MotorRegulationMode.MOTOR_SPEED, 0,
                                MotorRunState.RUNNING, 0);
                    }
                } catch (IOException e) {
                    timer.cancel();
                }
            }
        }, 0, 100);
        initiated = true;
    }

    public void stop() throws IOException {
//        setOutputState(0, 0, EnumSet.of(MotorMode.MOTORON),
//                MotorRegulationMode.MOTOR_SYNC, 0, MotorRunState.IDLE,
//                0);
        setOutputState(NXTPORT_B, 0, EnumSet.of(MotorMode.MOTORON),
                MotorRegulationMode.MOTOR_SYNC, 0, MotorRunState.IDLE,
                0);
        setOutputState(NXTPORT_C, 0, EnumSet.of(MotorMode.MOTORON),
                MotorRegulationMode.MOTOR_SYNC, 0, MotorRunState.IDLE,
                0);
    }

    /**
     * Set motor state
     *
     * @param leftSpeed
     *            Left motor speed, from -100 (full speed reverse) to 100 (full
     *            speed)
     * @param rightSpeed
     *            Right motor speed, from -100 (full speed reverse) to 100 (full
     *            speed)
     * @throws IOException
     */
    void setMotorState(int leftSpeed, int rightSpeed) throws IOException {
        if (!initiated) {
            initiate();
        }
        this.leftSpeed = leftSpeed;
        this.rightSpeed = rightSpeed;
    }

    private void setOutputState(int outputPort, int powerSetPoint,
            EnumSet<MotorMode> modeSet, MotorRegulationMode regulationMode,
            int turnRatio, MotorRunState runState, long tachoLimit)
            throws IOException {
        Assert.booleanExpression((outputPort >= 0 && outputPort <= 2)
                || outputPort == 0xFF);
        Assert.range(powerSetPoint, -100, 100);
        Assert.range(turnRatio, -100, 100);
        Assert.range(tachoLimit, 0, Integer.MAX_VALUE);

        // construct a bit field from the EnumSet
        int mode_bitfield = 0;
        for (MotorMode mode : modeSet) {
            mode_bitfield |= mode.toInt();
        }

        ByteBuffer buffer = IOUtils.createBuffer();
        buffer.put((byte) 0x80);
        buffer.put((byte) 0x04);
        buffer.put((byte) outputPort);
        buffer.put((byte) powerSetPoint);
        buffer.put((byte) mode_bitfield);
        buffer.put((byte) regulationMode.toInt());
        buffer.put((byte) turnRatio);
        buffer.put((byte) runState.toInt());
        buffer.putInt((int) tachoLimit);
        parent.sendMessage(buffer);
    }
    enum MotorMode {
        MOTORON(0x01), BRAKE(0x02), REGULATED(0x04);

        private final int mode;

        MotorMode(int mode) {
            this.mode = mode;
        }

        public int toInt() {
            return mode;
        }
    }

    private enum MotorRegulationMode {
        IDLE(0x00), MOTOR_SPEED(0x01), MOTOR_SYNC(0x02);

        private final int regulationMode;

        MotorRegulationMode(int regulationMode) {
            this.regulationMode = regulationMode;
        }

        public int toInt() {
            return regulationMode;
        }
    }

    private enum MotorRunState {
        IDLE(0x00), RAMPUP(0x10), RUNNING(0x20), RAMPDOWN(0x40);

        private final int motorRunState;

        MotorRunState(int motorRunState) {
            this.motorRunState = motorRunState;
        }

        public int toInt() {
            return motorRunState;
        }
    }

    private static class Assert {
        static void range(long value, long min, long max) {
            if (!(value >= min && value <= max)) {
                throw new IllegalArgumentException(Long.toString(value));
            }
        }

//        static void largerThan(long value, long min) {
//            if (value < min) {
//                throw new IllegalArgumentException(Long.toString(value));
//            }
//        }

        static void booleanExpression(boolean value) {
            if (!value) {
                throw new IllegalArgumentException();
            }
        }
    }

}
